#ifndef _CALC_H_
#define _CALC_H_
#include<immintrin.h>
#include<omp.h>
//#include<stdio.h>
#ifdef __cplusplus
extern "C"{	
#endif

typedef struct {
	int* 			rowArray;
	const int* 		rowOffset;
	int 			rowArraySize;
	const int* 		columnIndice;
	const double* 	S;
	const double*	valueNormalMatrix;
	double* 		Id;
}TaskMatrixInfoA;

typedef struct {
	const double * 	valueSpiceMatrix;//N*2
	const int* 		rowOffset; 		//n+1
	const int* 		columnIndice; 	//N

	double* 		A;	//N
	double*		 	S;	//nx1
	double*			R;	//nx1
	double* 		H;	//nx1
	const double* 	D;	//N*2
	double* 		IC;	//nx1
	double* 		IG;	//nx1
	double 		alpha;
	int * 			rowArray; //kx1
	int 			rowArraySize;
	void* 			hdl;  
}TaskMatrixInfoB;

int MIN(int i,int j){
	if(i>j)return j;
	else return i;
}
#define ROWNUM 2000
#define thread_num 8
void matrix_calc_taskA(TaskMatrixInfoA** listDataList,int N)
{
	#pragma omp parallel for num_threads(thread_num)schedule(guided)
	for (int i = 0 ;i < N; ++i)
	{	
		int row_array_size= listDataList[i]->rowArraySize;	
		TaskMatrixInfoA * list_Data_List=listDataList[i];
		const double *vNM = list_Data_List->valueNormalMatrix;
		const double *S = list_Data_List->S;
		const int *cI = list_Data_List->columnIndice;
		const int *rOf= list_Data_List->rowOffset;
		int *rA =list_Data_List->rowArray;
		double *Id = list_Data_List->Id;

		int left = row_array_size &0x03;
		int it;
		double tmp[4] __attribute__((aligned(32)));
		__m256d m1,m2,m3,m4;
		int node4[4];
		int jstart[4];
		int jend[4];
//前left行
		for ( it = 0;it<left;it++){
			const int node = list_Data_List->rowArray[it];
			int colum_num = list_Data_List->rowOffset[node + 1];
			double &id=list_Data_List->Id[node];
				for (int j = list_Data_List->rowOffset[node];j < colum_num;++j)	
				{   
					id += vNM[j]*S[cI[j]];
				}
		}
//后left行到row_array_size
		if (row_array_size<ROWNUM)
		{
			for (it = left ; it < row_array_size; it=it + 4)
			{
				node4[0]=rA[it];  node4[1]=rA[it+1];
				node4[2]=rA[it+2];node4[3]=rA[it+3];
				jstart[0]=rOf[node4[0]];jend[0]=rOf[node4[0]+1];
				jstart[1]=rOf[node4[1]];jend[1]=rOf[node4[1]+1];
				jstart[2]=rOf[node4[2]];jend[2]=rOf[node4[2]+1];
				jstart[3]=rOf[node4[3]];jend[3]=rOf[node4[3]+1];

				m1 = _mm256_set_pd(vNM[jstart[3]],vNM[jstart[2]],vNM[jstart[1]]	,vNM[jstart[0]]);
				m2 = _mm256_set_pd(S[cI[jstart[3]]],S[cI[jstart[2]]],S[cI[	jstart[1]]],S[cI[jstart[0]]]);
				m3 = _mm256_mul_pd(m1,m2);
				_mm256_store_pd(tmp,m3);
	
				for (int it2=0;it2<4;it2++){
					for(int tmpj = jstart[it2]+1;tmpj<jend[it2];tmpj++){
						tmp[it2]+=vNM[tmpj]*S[cI[tmpj]];	
					}
				}
				Id[node4[0]]+=tmp[0];	Id[node4[1]]+=tmp[1];
				Id[node4[2]]+=tmp[2];	Id[node4[3]]+=tmp[3];
			}
		}
		else //5000行的矩阵计算，每行大多数4~5个数据
		{
			for (it = left ; it < row_array_size; it=it + 4)
			{
				node4[0]=rA[it];  node4[1]=rA[it+1];
				node4[2]=rA[it+2];node4[3]=rA[it+3];
				jstart[0]=rOf[node4[0]];jend[0]=rOf[node4[0]+1];
				jstart[1]=rOf[node4[1]];jend[1]=rOf[node4[1]+1];
				jstart[2]=rOf[node4[2]];jend[2]=rOf[node4[2]+1];
				jstart[3]=rOf[node4[3]];jend[3]=rOf[node4[3]+1];
				//选最小值
				int min=MIN(MIN(jend[3]-jstart[3],jend[2]-jstart[2]),MIN(jend[1]-jstart[1],jend[0]-jstart[0]));
				m4 = _mm256_set1_pd(0);
				for(int mintmp=0;mintmp<min;mintmp++){
					m1 = _mm256_set_pd(vNM[jstart[3]+mintmp],vNM[jstart[2]+mintmp],vNM[jstart[1]+mintmp],vNM[jstart[0]+mintmp]	);
					m2 = _mm256_set_pd(S[cI[jstart[3]+mintmp]],S[cI[jstart[2]+mintmp]],S[cI[jstart[1]+mintmp]],S[cI[jstart[0]+mintmp]]);
					m3 = _mm256_mul_pd(m1,m2);
					m4 = _mm256_add_pd(m4,m3);
				}
				_mm256_store_pd(tmp,m4);
	
				for (int it2=0;it2<4;it2++){
					for(int tmpj = jstart[it2]+min;tmpj<jend[it2];tmpj++){
						tmp[it2]+=vNM[tmpj]*S[cI[tmpj]];	
					}
				}
				Id[node4[0]]+=tmp[0];	Id[node4[1]]+=tmp[1];
				Id[node4[2]]+=tmp[2];	Id[node4[3]]+=tmp[3];
			}
		}
	}
}

//Task B interface
void matrix_calc_taskB(TaskMatrixInfoB** listDataList,int N)
{
	#pragma omp parallel for num_threads(thread_num)schedule(guided)
	for (int i = 0;i<N;i++)
	{
		TaskMatrixInfoB * list_Data_List = listDataList[i];
		int row_array_size = list_Data_List->rowArraySize;
		const double alpha = list_Data_List->alpha;
		double* A = list_Data_List->A;
		const double* S = list_Data_List->S;
		const int* cI=list_Data_List->columnIndice;
		const double* vSM=list_Data_List->valueSpiceMatrix;
		int *rA = list_Data_List->rowArray;
		const int* rOf = list_Data_List->rowOffset;

		int left = row_array_size & 0x03;
		int it;


		for( it = 0;it < left;++it)
		{
			int node = rA[it];
			const int k1 = node << 1;
			int jend = rOf[node+1];
			double ig_tmp=0;
			double ic_tmp=0;

			for (int p = rOf[node];p < jend;++p){
				const int k = p << 1;
				double cond =vSM[k];
				double cap  =vSM[k+1];					
				ig_tmp += cond * S[cI[p]];
				ic_tmp += cap * S[cI[p]];
				//(9)
				A[p] = cond +  cap * alpha;
			}
 			//(7)(8)
 			list_Data_List->IG[node] +=ig_tmp; 
 			list_Data_List->IC[node] +=ic_tmp; 
			list_Data_List->R[node] = list_Data_List->D[k1]-ig_tmp;
			list_Data_List->H[node] = list_Data_List->D[k1+1]-ic_tmp;
		}
		/////////////计算后面4的倍数的行数
		double ig_tmp[4] __attribute__((aligned(32)));
		double ic_tmp[4] __attribute__((aligned(32)));
		__m256d mg,mc,ms,m3,m4g,m4c;
		int node4[4];
		int jstart[4];
		int jend[4];
		int k1[4];

		if(row_array_size<ROWNUM){
			for (it = left;it<row_array_size;it+=4){
				node4[0]=rA[it];  node4[1]=rA[it+1];
				node4[2]=rA[it+2];node4[3]=rA[it+3];
				jstart[0]=rOf[node4[0]];jend[0]=rOf[node4[0]+1];
				jstart[1]=rOf[node4[1]];jend[1]=rOf[node4[1]+1];
				jstart[2]=rOf[node4[2]];jend[2]=rOf[node4[2]+1];
				jstart[3]=rOf[node4[3]];jend[3]=rOf[node4[3]+1];
				k1[0]= node4[0]<<1;k1[1]= node4[1]<<1;
				k1[2]= node4[2]<<1;k1[3]= node4[3]<<1;
				//计算每一行的第一个乘积
				mg = _mm256_set_pd(
					vSM[jstart[3]<<1],vSM[jstart[2]<<1],
					vSM[jstart[1]<<1],vSM[jstart[0]<<1]);
				mc = _mm256_set_pd(
					vSM[(jstart[3]<<1)+1],vSM[(jstart[2]<<1)+1],
					vSM[(jstart[1]<<1)+1],vSM[(jstart[0]<<1)+1])	;
				ms = _mm256_set_pd(
					S[cI[jstart[3]]],S[cI[jstart[2]]],
					S[cI[jstart[1]]],S[cI[jstart[0]]]);
				m3 = _mm256_mul_pd(mg,ms);
				_mm256_store_pd(ig_tmp,m3);
				m3 = _mm256_mul_pd(mc,ms);
				_mm256_store_pd(ic_tmp,m3);
				//计算每行剩下的乘积
				A[jstart[0]]=vSM[jstart[0]<<1]+vSM[(jstart[0]<<1)+1]*alpha;
				A[jstart[1]]=vSM[jstart[1]<<1]+vSM[(jstart[1]<<1)+1]*alpha;
				A[jstart[2]]=vSM[jstart[2]<<1]+vSM[(jstart[2]<<1)+1]*alpha;
				A[jstart[3]]=vSM[jstart[3]<<1]+vSM[(jstart[3]<<1)+1]*alpha;
	
				for(int it2=0;it2<4;it2++){
					for(int tmpj = jstart[it2]+1;tmpj<jend[it2];tmpj++){
						ig_tmp[it2]+= vSM[tmpj<<1]*S[cI[tmpj]];
						ic_tmp[it2]+= vSM[(tmpj<<1)+1]*S[cI[tmpj]];
						A[tmpj]  =vSM[tmpj<<1]+vSM[(tmpj<<1)+1]*alpha; 
					}
				}
				list_Data_List->IG[node4[0]]+=ig_tmp[0];
				list_Data_List->IG[node4[1]]+=ig_tmp[1];
				list_Data_List->IG[node4[2]]+=ig_tmp[2];
				list_Data_List->IG[node4[3]]+=ig_tmp[3];
	
				list_Data_List->IC[node4[0]]+=ic_tmp[0];
				list_Data_List->IC[node4[1]]+=ic_tmp[1];
				list_Data_List->IC[node4[2]]+=ic_tmp[2];
				list_Data_List->IC[node4[3]]+=ic_tmp[3];	
				
				list_Data_List->R[node4[0]] =list_Data_List->D[	k1[0]]-ig_tmp[0];
				list_Data_List->R[node4[1]] =list_Data_List->D[	k1[1]]-ig_tmp[1];
				list_Data_List->R[node4[2]] =list_Data_List->D[	k1[2]]-ig_tmp[2];
				list_Data_List->R[node4[3]] =list_Data_List->D[	k1[3]]-ig_tmp[3];
	
				list_Data_List->H[node4[0]] =list_Data_List->D[	k1[0]+1]-ic_tmp[0];
				list_Data_List->H[node4[1]] =list_Data_List->D[	k1[1]+1]-ic_tmp[1];
				list_Data_List->H[node4[2]] =list_Data_List->D[	k1[2]+1]-ic_tmp[2];
				list_Data_List->H[node4[3]] =list_Data_List->D[	k1[3]+1]-ic_tmp[3];			
			}  
		}
		else
		{
			for (it = left;it<row_array_size;it+=4){
				node4[0]=rA[it];  node4[1]=rA[it+1];
				node4[2]=rA[it+2];node4[3]=rA[it+3];
				jstart[0]=rOf[node4[0]];jend[0]=rOf[node4[0]+1];
				jstart[1]=rOf[node4[1]];jend[1]=rOf[node4[1]+1];
				jstart[2]=rOf[node4[2]];jend[2]=rOf[node4[2]+1];
				jstart[3]=rOf[node4[3]];jend[3]=rOf[node4[3]+1];
				k1[0]= node4[0]<<1;k1[1]= node4[1]<<1;
				k1[2]= node4[2]<<1;k1[3]= node4[3]<<1;
				//计算每一行的第min个乘积
				int min=MIN(MIN(jend[3]-jstart[3],jend[2]-jstart[2]),MIN(jend[1]-jstart[1],jend[0]-jstart[0]));
//				int min=1;
				m4g = _mm256_set1_pd(0);
				m4c = _mm256_set1_pd(0);
				for(int mintmp=0;mintmp<min;mintmp++){					
					mg = _mm256_set_pd(
						vSM[(jstart[3]+mintmp)<<1],vSM[(jstart[2]+mintmp)<<1],
						vSM[(jstart[1]+mintmp)<<1],vSM[(jstart[0]+mintmp)<<1]);
					mc = _mm256_set_pd(
						vSM[((jstart[3]+mintmp)<<1)+1],vSM[((jstart[2]+mintmp)<<1)+1],
						vSM[((jstart[1]+mintmp)<<1)+1],vSM[((jstart[0]+mintmp)<<1)+1]);
					ms = _mm256_set_pd(
						S[cI[jstart[3]+mintmp]],S[cI[jstart[2]+mintmp]],
						S[cI[jstart[1]+mintmp]],S[cI[jstart[0]+mintmp]]);
					m3 = _mm256_mul_pd(mg,ms);
					m4g = _mm256_add_pd(m4g,m3);
					m3 = _mm256_mul_pd(mc,ms);
					m4c = _mm256_add_pd(m4c,m3);
					A[jstart[0]+mintmp]=vSM[jstart[0]+mintmp<<1]+vSM[(jstart[0]+mintmp<<1)+1]*alpha;
					A[jstart[1]+mintmp]=vSM[jstart[1]+mintmp<<1]+vSM[(jstart[1]+mintmp<<1)+1]*alpha;
					A[jstart[2]+mintmp]=vSM[jstart[2]+mintmp<<1]+vSM[(jstart[2]+mintmp<<1)+1]*alpha;
					A[jstart[3]+mintmp]=vSM[jstart[3]+mintmp<<1]+vSM[(jstart[3]+mintmp<<1)+1]*alpha;
				}
				_mm256_store_pd(ig_tmp,m4g);
				_mm256_store_pd(ic_tmp,m4c);
				//计算每行剩下的乘积
					
				for(int it2=0;it2<4;it2++){
					for(int tmpj = jstart[it2]+min;tmpj<jend[it2];tmpj++){
						ig_tmp[it2]+= vSM[tmpj<<1]*S[cI[tmpj]];
						ic_tmp[it2]+= vSM[(tmpj<<1)+1]*S[cI[tmpj]];
						A[tmpj]  =vSM[tmpj<<1]+vSM[(tmpj<<1)+1]*alpha; 
					}
				}
				list_Data_List->IG[node4[0]]+=ig_tmp[0];
				list_Data_List->IG[node4[1]]+=ig_tmp[1];
				list_Data_List->IG[node4[2]]+=ig_tmp[2];
				list_Data_List->IG[node4[3]]+=ig_tmp[3];
	
				list_Data_List->IC[node4[0]]+=ic_tmp[0];
				list_Data_List->IC[node4[1]]+=ic_tmp[1];
				list_Data_List->IC[node4[2]]+=ic_tmp[2];
				list_Data_List->IC[node4[3]]+=ic_tmp[3];	
				
				list_Data_List->R[node4[0]] =list_Data_List->D[	k1[0]]-ig_tmp[0];
				list_Data_List->R[node4[1]] =list_Data_List->D[	k1[1]]-ig_tmp[1];
				list_Data_List->R[node4[2]] =list_Data_List->D[	k1[2]]-ig_tmp[2];
				list_Data_List->R[node4[3]] =list_Data_List->D[	k1[3]]-ig_tmp[3];
	
				list_Data_List->H[node4[0]] =list_Data_List->D[	k1[0]+1]-ic_tmp[0];
				list_Data_List->H[node4[1]] =list_Data_List->D[	k1[1]+1]-ic_tmp[1];
				list_Data_List->H[node4[2]] =list_Data_List->D[	k1[2]+1]-ic_tmp[2];
				list_Data_List->H[node4[3]] =list_Data_List->D[	k1[3]+1]-ic_tmp[3];			
			}  

		}	
	}
}

#ifdef __cplusplus
}
#endif

#endif
